/*
 * imu.c
 *
 *  Created on: Apr 23, 2013
 *      Author: Daniel
 */

#include "DSP28x_Project.h"
#include "imu.h"


void InitIMU(void) {
	InitIMU_Offsets();
//	InitIMU_SMPLRT_DIV();
//	InitIMU_DLPF_FS();
//	InitIMU_AUX_BURST_ADDR();
	return;
}

void InitIMU_Offsets(void) {

	while(I2caRegs.I2CSTR.bit.BB == 1);	// wait until the bus is no longer busy

	I2caRegs.I2CFFTX.bit.TXFFRST = 0;	// reset the FIFO
	I2caRegs.I2CFFTX.bit.TXFFRST = 1;	// bring the FIFO out of reset

	I2caRegs.I2CSAR = IMU_I2C_Address;	// tell the I2C peripheral where we're transmitting
	I2caRegs.I2CCNT = 3;				// tell the I2C peripheral how many bytes we're transmitting

	I2caRegs.I2CDXR = IMU_ADDR_X_OFFS_USRH;
	I2caRegs.I2CDXR = IMU_INIT_X_OFFS_USRH;
	I2caRegs.I2CDXR = IMU_INIT_X_OFFS_USRL;

	I2caRegs.I2CMDR.all = 0x6E20;		// send the START condition

	while(I2caRegs.I2CSTR.bit.BB == 1);	// wait until the bus is no longer busy

	I2caRegs.I2CFFTX.bit.TXFFRST = 0;	// reset the FIFO
	I2caRegs.I2CFFTX.bit.TXFFRST = 1;	// bring the FIFO out of reset

	I2caRegs.I2CSAR = IMU_I2C_Address;	// tell the I2C peripheral where we're transmitting
	I2caRegs.I2CCNT = 3;				// tell the I2C peripheral how many bytes we're transmitting

	I2caRegs.I2CDXR = IMU_ADDR_Y_OFFS_USRH;
	I2caRegs.I2CDXR = IMU_INIT_Y_OFFS_USRH;
	I2caRegs.I2CDXR = IMU_INIT_Y_OFFS_URSL;

	I2caRegs.I2CMDR.all = 0x6E20;		// send the START condition

	while(I2caRegs.I2CSTR.bit.BB == 1);	// wait until the bus is no longer busy

	I2caRegs.I2CFFTX.bit.TXFFRST = 0;	// reset the FIFO
	I2caRegs.I2CFFTX.bit.TXFFRST = 1;	// bring the FIFO out of reset

	I2caRegs.I2CSAR = IMU_I2C_Address;	// tell the I2C peripheral where we're transmitting
	I2caRegs.I2CCNT = 3;				// tell the I2C peripheral how many bytes we're transmitting

	I2caRegs.I2CDXR = IMU_ADDR_Z_OFFS_USRH;
	I2caRegs.I2CDXR = IMU_INIT_Z_OFFS_USRH;
	I2caRegs.I2CDXR = IMU_INIT_Z_OFFS_USRL;

	I2caRegs.I2CMDR.all = 0x6E20;		// send the START condition

	return;
}

void InitIMU_SMPLRT_DIV(void) {

	I2caRegs.I2CSTR.all = I2C_CLR_NACK_BIT;		// clear the NACK bit
	while(I2caRegs.I2CSTR.bit.BB == 1);			// wait until the buss is no longer busy
	I2caRegs.I2CSAR = IMU_I2C_Address;			// tell the I2C peripheral where we're transmitting
	I2caRegs.I2CCNT = IMU_NUMBYTES_SMPLRT_DIV;	// tell the I2C peripheral how many bytes we're transmitting

	I2caRegs.I2CDXR = IMU_ADDR_SMPLRT_DIV;
	I2caRegs.I2CDXR = IMU_INIT_SMPLRT_DIV;

	I2caRegs.I2CMDR.all = 0x6E20;				// send the START condition

//	while(I2caRegs.I2CMDR.bit.STP == 1);		// wait for the transmission to finish
												// or for a NACK to occur
	return;
}

void InitIMU_DLPF_FS(void) {
	I2caRegs.I2CSTR.all = I2C_CLR_NACK_BIT;		// clear the NACK bit
	while(I2caRegs.I2CSTR.bit.BB == 1);			// wait until the buss is no longer busy
	I2caRegs.I2CSAR = IMU_I2C_Address;			// tell the I2C peripheral where we're transmitting
	I2caRegs.I2CCNT = IMU_NUMBYTES_DLPF_FS;		// tell the I2C peripheral how many bytes we're transmitting

	I2caRegs.I2CDXR = IMU_ADDR_DLPF_FS;
	I2caRegs.I2CDXR = IMU_INIT_DLPF_FS;

	I2caRegs.I2CMDR.all = 0x6E20;				// send the START condition

	return;
}

void InitIMU_AUX_BURST_ADDR(void) {
	while(I2caRegs.I2CSTR.bit.BB == 1);			// wait until the buss is no longer busy

	I2caRegs.I2CSAR = IMU_I2C_Address;			// tell the I2C peripheral where we're transmitting
	I2caRegs.I2CCNT = IMU_NUMBYTES_AUX_BURST_ADDR;	// tell the I2C peripheral how many bytes we're transmitting

	I2caRegs.I2CDXR = IMU_ADDR_AUX_BURST_ADDR;
	I2caRegs.I2CDXR = IMU_INIT_AUX_BURST_ADDR;

	I2caRegs.I2CMDR.all = 0x6E20;				// send the START condition

	return;
}

void IMU_read_offsets(Uint16 results[]) {
	Uint16 i;

	while(I2caRegs.I2CSTR.bit.BB == 1);

	I2caRegs.I2CFFTX.bit.TXFFRST = 0;	// reset the FIFO
	I2caRegs.I2CFFTX.bit.TXFFRST = 1;	// bring the FIFO out of reset

	I2caRegs.I2CSAR = IMU_I2C_Address;
	I2caRegs.I2CCNT = 1;

	I2caRegs.I2CDXR = IMU_ADDR_X_OFFS_USRH;

	I2caRegs.I2CMDR.all = 0x2620;				// issue a START command (no stop)

	while(I2caRegs.I2CSTR.bit.BB == 1);

	I2caRegs.I2CSAR = IMU_I2C_Address;
	I2caRegs.I2CCNT = 2;						// send and NACK after two bytes
	I2caRegs.I2CFFRX.bit.RXFFST = 2;			// throw an interrupt flag when the FIFO has two bytes
	I2caRegs.I2CMDR.all = 0x2C20;				// issue a restart command

	while(I2caRegs.I2CFFRX.bit.RXFFINT != 1);	// wait for the interrupt flag to be thrown

	I2caRegs.I2CFFRX.bit.RXFFINTCLR = 1;		// clear the interrupt bit so that we can use it again later

	for(i = 0; i < 2; i++) {
		results[i] = I2caRegs.I2CDRR;
	}

	return;
}


void IMU_read_all(Uint16 results[]) {
	I2caRegs.I2CSTR.all = I2C_CLR_NACK_BIT;
	while(I2caRegs.I2CSTR.bit.BB == 1);
	I2caRegs.I2CSAR = IMU_I2C_Address;
	I2caRegs.I2CCNT = 1;

	I2caRegs.I2CDXR = IMU_ADDR_TEMP_OUT_H;

	I2caRegs.I2CMDR.all = 0x2620;				// issue a START command (no stop)

	while(I2caRegs.I2CSTR.bit.BB == 1);
	I2caRegs.I2CSAR = IMU_I2C_Address;
	I2caRegs.I2CCNT = IMU_NUMBYTES_READ_ALL;
	I2caRegs.I2CMDR.all = 0x2C20;				// issue a restart command

	while(I2caRegs.I2CSTR.bit.NACKSNT == 0);	// wait for the transmission to finish

	return;
}
